#include <stdio.h>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Odometry.h>
#include <std_msgs/Float64MultiArray.h>
#include <webots_ros/Int32Stamped.h>
#include <webots_ros/Float64Stamped.h>
#include <webots_ros/get_int.h>
#include <webots_ros/robot_get_device_list.h>

#include "ros/ros.h"
#include <sensor_msgs/Image.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/NavSatFix.h>
#include <signal.h>
#include <std_msgs/String.h>
#include<tf/transform_broadcaster.h>
 
#include <webots_ros/set_float.h>
#include <webots_ros/set_int.h>
 


#define TIME_STEP 32
#define MOTOR_POSI_ADD 0.3
#define WHEEL_RADIUS 0.05
#define LX 0.156 // half axlespacing, longitudinal distance from robot's COM to wheel [m].
#define LY 0.176  // half wheelspacing, lateral distance from robot's COM to wheel [m].
#define TRACK_TIME_STEP 0.1
#define pi 3.14159265358

void quit(int sig);

class RosBasic
{
public:
    RosBasic(ros::NodeHandle &nh);
    ~RosBasic();

    // Member Functions
    void control_mode_choose();
    void ros_quit();
    void keyboard_control_VE();
    void Twist_control_VE();
    void VE_mapping_PE();

    double Limit_angular(double th);
    void print_GPS_values();

    void velCallback(const geometry_msgs::Twist::ConstPtr &values);
    void controllerNameCallback(const std_msgs::String::ConstPtr &name);
    void keyboardCallback(const webots_ros::Int32Stamped::ConstPtr &value);
    void GPSCallback(const geometry_msgs::PointStamped::ConstPtr &values);
    void GPSSpeedCallback(const webots_ros::Float64Stamped::ConstPtr &value);
    void imuCallback(const sensor_msgs::Imu::ConstPtr &values);
    void controller2Callback(const geometry_msgs::Point::ConstPtr &values);

    // Member Properties
    ros::NodeHandle n;
    std::string controllerName;
    std::string nodeName;
    int controllerCount;
    int controlMode;
    std::vector<std::string> controllerList;

    double w1Position = 0;
    double w2Position = 0;
    double w3Position = 0;
    double w4Position = 0;
    double GPSValues[3];
    double WheelVelocity[4];

    double x;
    double y;
    double th;
    double vx;
    double vy;
    double vth;

    // double q_x;
    // double q_y;
    // double q_th;
    double q_vx;
    double q_vy;
    double q_vth;


    ros::ServiceClient timeStepClient;
    webots_ros::set_int timeStepSrv;
    ros::ServiceClient enableKeyboardClient;
    webots_ros::set_int enableKeyboardSrv;
    ros::ServiceClient enableGPSClient;
    webots_ros::set_int enableGPSSrv;
    ros::ServiceClient enableImuClient;
    webots_ros::set_int enableImuSrv;

    ros::ServiceClient Wheel1SetPositionClient;
    ros::ServiceClient Wheel2SetPositionClient;
    ros::ServiceClient Wheel3SetPositionClient;
    ros::ServiceClient Wheel4SetPositionClient;
    webots_ros::set_float Wheel1SetPositionSrv;
    webots_ros::set_float Wheel2SetPositionSrv;
    webots_ros::set_float Wheel3SetPositionSrv;
    webots_ros::set_float Wheel4SetPositionSrv;

    ros::ServiceClient W1SetVelocityClient;
    ros::ServiceClient W2SetVelocityClient;
    ros::ServiceClient W3SetVelocityClient;
    ros::ServiceClient W4SetVelocityClient;
    webots_ros::set_float W1SetVelocitySrv;
    webots_ros::set_float W2SetVelocitySrv;
    webots_ros::set_float W3SetVelocitySrv;
    webots_ros::set_float W4SetVelocitySrv;
};
